/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#if 0

//#include "windows.h"

//#define ALPHA_EXPANSION

#ifndef ALPHA_EXPANSION
#define ALPHA_BETA_EXCHANGE
#endif

#define MAX_LABEL 20

#define CV_MODULE(xxx) \
    ( (xxx) < 0 ? -(xxx) : (xxx) )

#define CV_MAX3(xxx1,xxx2,xxx3) \
    ( (xxx1) > (xxx2) && (xxx1) > (xxx3) ? (xxx1) : \
        (xxx2) > (xxx3) ? (xxx2) : (xxx3) )

#define CV_MIN2(xxx1,xxx2) \
    ( (xxx1) < (xxx2) ? (xxx1) : (xxx2) )

#define getSizeForGraph(xxxType) \
    ( sizeof(xxxType) < 8 ? 8 : sizeof(xxxType) + 4 - sizeof(xxxType) % 4 )

#define INT_INFINITY 1000000000
#define MAX_DIFFERENCE 10


// struct Vertex is used for storing vertices of graph
//      coord       - coordinate corresponding pixel on the real image line
struct Vertex {
	CvGraphVtx vtx;
	int coord;
};

// struct Edge is used for storing edges of graph
//      weight      - weight of the edge ( maximum flow via the edge )
//      flow        - current flow via the edge
//      srcVtx      - coordinate of source vertex on the real image line
//      destVtx     - coordinate of destination vertex on the real image line
struct Edge {
	CV_GRAPH_EDGE_FIELDS()
	int weight;
	int flow;
	int srcVtx;
	int destVtx;
};

// function vFunc is energy function which determines the difference
//   between two labels ( alpha and beta )
//      alpha       - label number one
//      beta        - label number two
inline int vFunc( int alpha, int beta ) {
	if ( alpha == beta ) {
		return 0;
	} else {
		return /*1*//*5*/10;
	}
}

// function dFunc is energy function which determines energy of interaction
//   between pixel ( having coordinate xCoord ) and label
//          leftLine        - line of left image
//          rightLine       - line of right image
//          xCoord          - coordinate of pixel on the left image
//          label           - label corresponding to the pixel
//          width           - width of the image line in pixels
inline int dFunc( unsigned char* leftLine,
				  unsigned char* rightLine,
				  int xCoord,
				  int label,
				  int width) {
	assert( xCoord >= 0 && xCoord < width );
	int r, g, b;
	int yCoord = xCoord + label;

	if ( yCoord >= width ) {
		yCoord = width;
	}
	if ( yCoord < 0 ) {
		yCoord = 0;
	}

	r = leftLine[ 3 * xCoord     ] - rightLine[ 3 * yCoord     ];
	g = leftLine[ 3 * xCoord + 1 ] - rightLine[ 3 * yCoord + 1 ];
	b = leftLine[ 3 * xCoord + 2 ] - rightLine[ 3 * yCoord + 2 ];

	r = CV_MODULE( r );
	g = CV_MODULE( g );
	b = CV_MODULE( b );

	return CV_MAX3( r, g, b );
}

// function allocTempMem allocates all temporary memory needed for work
//   of some function
//      memPtr          - pointer to pointer to the large block of memory
//      verticesPtr     - pointer to pointer to block of memory for
//                        temporary storing vertices
//      width           - width of line in pixels
void allocTempMem( int** memPtr,
				   int** verticesPtr,
				   int width ) {
	int* tempPtr = ( int* ) malloc( ( width + 2 ) * 7 * sizeof( int ) );
	*verticesPtr = tempPtr;
	*memPtr = *verticesPtr + width + 2;
}

// function freeTempMem frees all allocated by allocTempMem function memory
//      memPtr          - pointer to pointer to the large block of memory
//      verticesPtr     - pointer to pointer to block of memory for
//                        temporary storing vertices
void freeTempMem( int** memPtr,
				  int** verticesPtr ) {
	free( ( void* )( *verticesPtr ) );
	*verticesPtr = NULL;
	*memPtr = NULL;
}

// function makeGraph creates initial graph to find maximum flow in it
//      graphPtr        - pointer to pointer to CvGraph structure to be filled
//      leftLine        - pointer to the left image line
//      rightLine       - pointer to the right image line
//      alpha           - label number one for doing exchange
//      beta            - label number two for doing exchange
//      corr            - pointer to array of correspondences ( each element
//                        of array includes disparity of pixel on right image
//                        for pixel each on left image ). This pointer direct
//                        to correspondence ofr one line only
//      width           - width of image lines in pixels
//      storage         - pointer to CvMemStorage structure which contains
//                        memory storage
void makeGraph( CvGraph** graphPtr,
				unsigned char* leftLine,
				unsigned char* rightLine,
				int alpha,
				int beta,
				int* corr,
				int width,
				CvMemStorage* storage ) {
	int i;

	if ( *graphPtr )  {
		cvClearGraph( *graphPtr );
	}
	/*else {*/
	*graphPtr = cvCreateGraph( CV_SEQ_KIND_GRAPH | CV_GRAPH_FLAG_ORIENTED,
							   sizeof( CvGraph ),
							   getSizeForGraph( Vertex ),
							   getSizeForGraph( Edge ),
							   storage );
	/*}*/

	CvGraph* graph = *graphPtr;

#ifdef ALPHA_BETA_EXCHANGE

	CvGraphVtx* newVtxPtr;
	for ( i = 0; i < width; i ++ ) {
		if ( corr[i] == alpha || corr[i] == beta ) {
			cvGraphAddVtx( graph, NULL, &newVtxPtr );
			( ( Vertex* )newVtxPtr ) -> coord = i;
		}
	} /* for( i = 0; i < width; i ++ ) */
	cvGraphAddVtx( graph, NULL, &newVtxPtr );
	if ( newVtxPtr ) {
		( ( Vertex* )newVtxPtr ) -> coord = -2;    /* adding alpha vertex */
	}
	cvGraphAddVtx( graph, NULL, &newVtxPtr );
	if ( newVtxPtr ) {
		( ( Vertex* )newVtxPtr ) -> coord = -1;    /* adding beta vertex */
	}

	int alphaVtx = graph -> total - 2;
	int betaVtx = graph -> total - 1;
	CvGraphEdge* newEdgePtr;
	CvGraphVtx* vtxPtr;
	if ( graph -> total > 2 ) {
		for ( i = 0; i < alphaVtx; i ++ ) {
			vtxPtr = cvGetGraphVtx( graph, i );

			/* adding edge oriented from alpha vertex to current vertex */
			cvGraphAddEdge( graph, alphaVtx, i, NULL, &newEdgePtr );
			( ( Edge* )newEdgePtr ) -> weight = dFunc( leftLine,
												rightLine,
												( ( Vertex* )vtxPtr ) -> coord,
												alpha,
												width );
			( ( Edge* )newEdgePtr ) -> flow = 0;
			if ( i != 0 ) {
				CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i - 1 );
				/* if vertices are neighbours */
				if ( ( ( Vertex* )tempVtxPtr ) -> coord + 1 ==
						( ( Vertex* )vtxPtr ) -> coord ) {
					( ( Edge* )newEdgePtr ) -> weight +=
						vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
							   alpha );
					/* adding neighbour edge oriented from current vertex
					   to the previous one */
					CvGraphEdge* tempEdgePtr;
					cvGraphAddEdge( graph, i, i - 1, NULL, &tempEdgePtr );
					( ( Edge* )tempEdgePtr ) -> weight = vFunc( alpha, beta );
					( ( Edge* )tempEdgePtr ) -> flow = 0;
					( ( Edge* )tempEdgePtr ) -> srcVtx =
						( ( Vertex* )vtxPtr ) -> coord;
					( ( Edge* )tempEdgePtr ) -> destVtx =
						( ( Vertex* )tempVtxPtr ) -> coord;
				}
			} /* if( i != 0 ) */
			if ( i != alphaVtx - 1 ) {
				CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i + 1 );
				/* if vertices are neighbours */
				if ( ( ( Vertex* )tempVtxPtr ) -> coord - 1 ==
						( ( Vertex* )vtxPtr ) -> coord ) {
					( ( Edge* )newEdgePtr ) -> weight +=
						vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
							   alpha );
					/* adding neighbour edge oriented from current vertex
					   to the next one */
					CvGraphEdge* tempEdgePtr;
					cvGraphAddEdge( graph, i, i + 1, NULL, &tempEdgePtr );
					( ( Edge* )tempEdgePtr ) -> weight = vFunc( alpha, beta );
					( ( Edge* )tempEdgePtr ) -> flow = 0;
					( ( Edge* )tempEdgePtr ) -> srcVtx =
						( ( Vertex* )vtxPtr ) -> coord;
					( ( Edge* )tempEdgePtr ) -> destVtx =
						( ( Vertex* )tempVtxPtr ) -> coord;
				}
			} /* if( i != alphaVtx - 1 ) */
			( ( Edge* )newEdgePtr ) -> flow = 0;
			( ( Edge* )newEdgePtr ) -> srcVtx = -1; /* source vertex is alpha
                                                       vertex */
			( ( Edge* )newEdgePtr ) -> destVtx = ( ( Vertex* )vtxPtr ) -> coord;

			/* adding edge oriented from current vertex to beta vertex */
			cvGraphAddEdge( graph, i, betaVtx, NULL, &newEdgePtr );
			( ( Edge* )newEdgePtr ) -> weight = dFunc( leftLine,
												rightLine,
												( ( Vertex* )vtxPtr ) -> coord,
												beta,
												width );
			( ( Edge* )newEdgePtr ) -> flow = 0;
			if ( i != 0 ) {
				CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i - 1 );
				/* if vertices are neighbours */
				if ( ( ( Vertex* )tempVtxPtr ) -> coord + 1 ==
						( ( Vertex* )vtxPtr ) -> coord ) {
					( ( Edge* )newEdgePtr ) -> weight +=
						vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
							   beta );
				}
			} /* if( i != 0 ) */
			if ( i != alphaVtx - 1 ) {
				CvGraphVtx* tempVtxPtr = cvGetGraphVtx( graph, i + 1 );
				/* if vertices are neighbours */
				if ( ( ( Vertex* )tempVtxPtr ) -> coord - 1 ==
						( ( Vertex* )vtxPtr ) -> coord ) {
					( ( Edge* )newEdgePtr ) -> weight +=
						vFunc( corr[ ( ( Vertex* )tempVtxPtr ) -> coord ],
							   beta );
				}
			} /* if( i != alphaVtx - 1 ) */
			( ( Edge* )newEdgePtr ) -> flow = 0;
			( ( Edge* )newEdgePtr ) -> srcVtx =
				( ( Vertex* )vtxPtr ) -> coord;
			( ( Edge* )newEdgePtr ) -> destVtx = -2; /* destination vertex is
                                                        beta vertex */

		} /* for( i = 0; i < graph -> total - 2; i ++ ) */

	} /* if( graph -> total > 2 ) */

#endif /* #ifdef ALPHA_BETA_EXCHANGE */

#ifdef ALPHA_EXPANSION
#endif /* #ifdef ALPHA_EXPANSION */

} /* makeGraph */

// function makeHelpGraph creates help graph using initial graph
//      graph           - pointer to initial graph ( represented by CvGraph
//                        structure )
//      hlpGraphPtr     - pointer to pointer to new help graph
//      storage         - pointer to CvStorage structure
//      mem             - pointer to memory allocated by allocTempMem function
//      vertices        - pointer to memory allocated by allocTempMem function
//      verticesCountPtr- pointer to value containing number of vertices
//                        in vertices array
//      width           - width of image line in pixels
int makeHelpGraph( CvGraph* graph,
				   CvGraph** hlpGraphPtr,
				   CvMemStorage* storage,
				   int* mem,
				   int* vertices,
				   int* verticesCountPtr,
				   int width ) {
	int u, v;
	int* order = mem;
	int* lengthArr = order + width + 2;
	int s = graph -> total - 2; /* source vertex */
	int t = graph -> total - 1; /* terminate vertex */
	int orderFirst;
	int orderCount;
	int& verticesCount = *verticesCountPtr;
	CvGraph* hlpGraph;

	if ( *hlpGraphPtr )  {
		cvClearGraph( *hlpGraphPtr );
	} else {
		*hlpGraphPtr = cvCreateGraph( CV_SEQ_KIND_GRAPH |
									  CV_GRAPH_FLAG_ORIENTED,
									  sizeof( CvGraph ),
									  getSizeForGraph( Vertex ),
									  getSizeForGraph( Edge ),
									  storage );
	}

	hlpGraph = *hlpGraphPtr;

	/* initialization */
	for ( u = 0; u < graph -> total; u ++ ) {
		lengthArr[ u ] = INT_INFINITY;
		cvGraphAddVtx( hlpGraph, NULL, NULL );
	} /* for( u = 0; u < graph -> total - 1; u ++ ) */

	orderFirst = 0;
	orderCount = 0;
	verticesCount = 0;
	lengthArr[ s ] = 0;

	/* add vertex s to order */
	order[ orderCount ] = s;
	orderCount ++;

	while ( orderCount != orderFirst ) {
		/* getting u from order */
		u = order[ orderFirst ];
		orderFirst ++;

		/* adding u to vertex array */
		vertices[ verticesCount ] = u;
		verticesCount ++;

		int ofs;
		CvGraphVtx* graphVtx = cvGetGraphVtx( graph, u );

		/* processing all vertices outgoing from vertex u */
		CvGraphEdge* graphEdge = graphVtx -> first;
		while ( graphEdge ) {
			int tempVtxIdx = cvGraphVtxIdx( graph, graphEdge -> vtx[1] );
			ofs = tempVtxIdx == u;
			if ( !ofs ) {
				v = tempVtxIdx;

				CvGraphEdge* tempGraphEdge = cvFindGraphEdge( graph, u, v );
				if ( ( lengthArr[ u ] < lengthArr[ v ] )
						&& ( lengthArr[ v ] <= lengthArr[ t ] )
						&& ( ( ( Edge* )tempGraphEdge ) -> flow <
							 ( ( Edge* )tempGraphEdge ) -> weight ) ) {
					if ( lengthArr[ v ] == INT_INFINITY ) {
						/* adding vertex v to order */
						order[ orderCount ] = v;
						orderCount ++;

						lengthArr[ v ] = lengthArr[ u ] + 1;
						CvGraphEdge* tempGraphEdge2;

						cvGraphAddEdge( hlpGraph, u, v, NULL, &tempGraphEdge2 );
						( ( Edge* )tempGraphEdge2 ) -> flow = 0;

						( ( Edge* )tempGraphEdge2 ) -> weight =
							( ( Edge* )tempGraphEdge ) -> weight -
							( ( Edge* )tempGraphEdge ) -> flow;

					} /* if( length[ v ] == INT_INFINITY ) */

				} /* if( ( lengthArr[ u ] < lengthArr[ v ] ) ... */

			} /* if( !ofs ) */

			graphEdge = graphEdge -> next[ ofs ];

		} /* while( graphEdge ) */

		/* processing all vertices incoming to vertex u */
		graphEdge = graphVtx -> first;
		while ( graphEdge ) {
			int tempVtxIdx = cvGraphVtxIdx( graph, graphEdge -> vtx[1] );
			ofs = tempVtxIdx == u;
			if ( ofs ) {
				tempVtxIdx = cvGraphVtxIdx( graph, graphEdge -> vtx[0] );
				v = tempVtxIdx;

				CvGraphEdge* tempGraphEdge = cvFindGraphEdge( graph, v, u );
				if ( ( lengthArr[ u ] < lengthArr[ v ] )
						&& ( lengthArr[ v ] <= lengthArr[ t ] )
						&& ( ( ( Edge* )tempGraphEdge ) -> flow > 0 ) ) {
					if ( lengthArr[ v ] == INT_INFINITY ) {
						/* adding vertex v to order */
						order[ orderCount ] = v;
						orderCount ++;

						lengthArr[ v ] = lengthArr[ u ] + 1;
						CvGraphEdge* tempGraphEdge3 = cvFindGraphEdge( hlpGraph, u, v );

						if ( tempGraphEdge3 == NULL ||
								( ( Edge* )tempGraphEdge3 ) -> weight == 0 ) {
							CvGraphEdge* tempGraphEdge2;
							cvGraphAddEdge( hlpGraph, u, v, NULL,
											&tempGraphEdge2 );
							( ( Edge* )tempGraphEdge2 ) -> flow = 0;
							( ( Edge* )tempGraphEdge2 ) -> weight = 0;
						} /* if( tempGraphEdge3 == NULL || ... */

						( ( Edge* )tempGraphEdge3 ) -> weight +=
							( ( Edge* )tempGraphEdge ) -> flow;

					} /* if( length[ v ] == INT_INFINITY ) */

				} /* if( ( lengthArr[ u ] < lengthArr[ v ] ) ... */

			} /* if( ofs ) */

			graphEdge = graphEdge -> next[ ofs ];

		} /* while( graphEdge ) */

	} /* while( orderCount != orderFirst ) */

	int i;
	for ( i = 0; i < hlpGraph -> total - 2; i ++ ) {
		CvGraphVtx* hlpGraphVtxTemp = cvGetGraphVtx( hlpGraph, i );
		if ( hlpGraphVtxTemp ) {
			if ( !hlpGraphVtxTemp -> first ) {
				cvGraphRemoveVtxByPtr( hlpGraph, hlpGraphVtxTemp );
			}
		}
	} /* for( i = 0; i < hlpGraph -> total - 2; i ++ ) */

	return lengthArr[ t ];

} /* makeHelpGraph */

// function makePseudoMaxFlow increases flow in graph by using hlpGraph
//      graph           - pointer to initial graph
//      hlpGraph        - pointer to help graph
//      vertices        - pointer to vertices array
//      verticesCount   - number of vertices in vertices array
//      mem             - pointer to memory allocated by allocTempMem function
//      width           - width of image line in pixels
void makePseudoMaxFlow( CvGraph* graph,
						CvGraph* hlpGraph,
						int* vertices,
						int verticesCount,
						int* mem,
						int width ) {
	int stekCount;
	int orderFirst;
	int orderCount;
	int i;
	int v, u;
	int* stek = mem;
	int* order = stek + width + 2;
	int* incomFlow = order + width + 2;
	int* outgoFlow = incomFlow + width + 2;
	int* flow = outgoFlow + width + 2;
	int* cargo = flow + width + 2;
	int s = graph -> total - 2; /* source vertex */
	int t = graph -> total - 1; /* terminate vertex */
	int realVerticesCount = verticesCount;

	stekCount = 0;

	for ( i = 0; i < verticesCount; i ++ ) {
		v = vertices[ i ];

		incomFlow[ v ] = outgoFlow[ v ] = 0;

		if ( v == s ) {
			incomFlow[ v ] = INT_INFINITY;
		} /* if( v == s ) */
		else {
			CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
			CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
			int ofs;

			while ( hlpGraphEdge ) {
				int vtxIdx = cvGraphVtxIdx( hlpGraph,
											hlpGraphEdge -> vtx[1] );
				ofs = vtxIdx == v;

				if ( ofs ) {
					incomFlow[ v ] += ( ( Edge* )hlpGraphEdge ) -> weight;
				} /* if( ofs ) */

				hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
			} /* while( hlpGraphEdge ) */

		} /* if( v == s ) else */

		if ( v == t ) {
			outgoFlow[ v ] = INT_INFINITY;
		} /* if( v == t ) */
		else {
			CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
			CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
			int ofs;

			while ( hlpGraphEdge ) {
				int vtxIdx = cvGraphVtxIdx( hlpGraph,
											hlpGraphEdge -> vtx[1] );
				ofs = vtxIdx == v;

				if ( !ofs ) {
					outgoFlow[ v ] += ( ( Edge* )hlpGraphEdge ) -> weight;
				} /* if( ofs ) */

				hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
			} /* while( hlpGraphEdge ) */

		} /* if( v == t ) else */

		flow[ v ] = CV_MIN2( incomFlow[ v ], outgoFlow[ v ] );

		if ( !flow[ v ] ) {
			stek[ stekCount ] = v;
			stekCount ++;
		} /* if( !flow[ v ] ) */

	} /* for( i = 0; i < verticesCount; i ++ ) */

	for ( i = 0; i < verticesCount; i ++ ) {
		v = vertices[ i ];
		cargo[ v ] = 0;
	} /* for( i = 0; i < verticesCount; i ++ ) */

	while ( realVerticesCount > 2 ) {
		/* deleting all vertices included in stek */
		while ( stekCount ) {
			v = stek[ stekCount - 1 ];
			stekCount --;

			/* deleting edges incoming to v and outgoing from v */
			int ofs;
			CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
			CvGraphEdge* hlpGraphEdge;
			if ( hlpGraphVtx ) {
				hlpGraphEdge = hlpGraphVtx -> first;
			} else {
				hlpGraphEdge = NULL;
			}
			while ( hlpGraphEdge ) {
				CvGraphVtx* hlpGraphVtx2 = hlpGraphEdge -> vtx[ 1 ];
				int hlpGraphVtxIdx2 = cvGraphVtxIdx( hlpGraph,
													 hlpGraphVtx2 );
				ofs = hlpGraphVtxIdx2 == v;

				if ( ofs ) {
					/* hlpGraphEdge is incoming edge */
					CvGraphVtx* hlpGraphVtx3 = hlpGraphEdge -> vtx[0];
					u = cvGraphVtxIdx( hlpGraph,
									   hlpGraphVtx3 );
					outgoFlow[ u ] -= ( ( Edge* )hlpGraphEdge ) -> weight
									  - ( ( Edge* )hlpGraphEdge ) -> flow;
					cvGraphRemoveEdgeByPtr( hlpGraph,
											hlpGraphVtx3,
											hlpGraphVtx2 );
					if ( flow[ u ] != 0 ) {
						flow[ u ] = CV_MIN2( incomFlow[u], outgoFlow[u] );
						if ( flow[ u ] == 0 ) {
							stek[ stekCount ] = u;
							stekCount ++;
						}
					}
				} /* if( ofs ) */
				else {
					/* hlpGraphEdge is outgoing edge */
					CvGraphVtx* hlpGraphVtx3 = hlpGraphEdge -> vtx[1];
					int u = cvGraphVtxIdx( hlpGraph,
										   hlpGraphVtx3 );
					incomFlow[ u ] -= ( ( Edge* )hlpGraphEdge ) -> weight
									  - ( ( Edge* )hlpGraphEdge ) -> flow;
					cvGraphRemoveEdgeByPtr( hlpGraph,
											hlpGraphVtx2,
											hlpGraphVtx3 );
					if ( flow[ u ] != 0 ) {
						flow[ u ] = CV_MIN2( incomFlow[u], outgoFlow[u] );
						if ( flow[ u ] == 0 ) {
							stek[ stekCount ] = u;
							stekCount ++;
						}
					}
				} /* if( ofs ) else */

				hlpGraphEdge = hlpGraphEdge -> next[ ofs ];

			} /* while( hlpGraphEdge ) */

			/* deleting vertex v */
			cvGraphRemoveVtx( hlpGraph, v );
			realVerticesCount --;

		} /* while( stekCount ) */

		if ( realVerticesCount > 2 ) { /* the flow is not max still */
			int p = INT_INFINITY;
			int r = -1;
			CvGraphVtx* graphVtx;

			if ( realVerticesCount == 3 ) {
				r = r;
			}
			for ( i = 0; i < hlpGraph -> total - 2; i ++ ) {
				graphVtx = cvGetGraphVtx( hlpGraph, i );
				if ( graphVtx ) {
					v = cvGraphVtxIdx( hlpGraph, graphVtx );
					if ( flow[ v ] < p ) {
						r = v;
						p = flow[ v ];
					}
				}

			} /* for( i = 0; i < hlpGraph -> total - 2; i ++ ) */

			/* building of size p flow from r to t */
			orderCount = orderFirst = 0;
			order[ orderCount ] = r;
			orderCount ++;

			v = order[ orderFirst ];
			orderFirst ++;

			cargo[ r ] = p;
			do { /* while( v != t ) */
				incomFlow[ v ] -= cargo[ v ];
				outgoFlow[ v ] -= cargo[ v ];
				flow[ v ] -= cargo[ v ];

				if ( flow[ v ] == 0 ) {
					stek[ stekCount ] = v;
					stekCount ++;
				}

				if ( v == t ) {
					cargo[ v ] = p;
				} else {
					int ofs;
					CvGraphVtx* hlpGraphVtx2;
					CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
					CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
					CvGraphEdge* hlpGraphEdge2 = NULL;

					while ( hlpGraphEdge && cargo[ v ] > 0 ) {
						hlpGraphVtx2 = hlpGraphEdge -> vtx[ 1 ];
						u = cvGraphVtxIdx( hlpGraph, hlpGraphVtx2 );
						ofs = u == v;

						if ( !ofs ) {
							if ( cargo[ u ] == 0 ) {
								order[ orderCount ] = u;
								orderCount ++;
							}
							int delta = ( ( Edge* )hlpGraphEdge ) -> weight
										- ( ( Edge* )hlpGraphEdge ) -> flow;
							delta = CV_MIN2( cargo[ v ], delta );
							( ( Edge* )hlpGraphEdge ) -> flow += delta;
							cargo[ v ] -= delta;
							cargo[ u ] += delta;
							if ( ( ( Edge* )hlpGraphEdge ) -> weight ==
									( ( Edge* )hlpGraphEdge ) -> flow ) {
								/* deleting hlpGraphEdge */
								hlpGraphEdge2 = hlpGraphEdge -> next[ ofs ];
								CvGraphEdge* graphEdge =
									cvFindGraphEdge( graph, v, u );
								( ( Edge* )graphEdge ) -> flow +=
									( ( Edge* )hlpGraphEdge ) -> flow;
								cvGraphRemoveEdgeByPtr( hlpGraph,
														hlpGraphEdge -> vtx[0],
														hlpGraphEdge -> vtx[1] );
							}
						} /* if( !ofs ) */

						if ( hlpGraphEdge2 ) {
							hlpGraphEdge = hlpGraphEdge2;
							hlpGraphEdge2 = NULL;
						} else {
							hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
						}
					} /* while( hlpGraphEdge && cargo[ v ] > 0 ) */

				} /* if( v == t ) else */

				v = order[ orderFirst ];
				orderFirst ++;

			} while ( v != t ); /* do */

			/* building of size p flow from s to r */
			orderCount = orderFirst = 0;
			order[ orderCount ] = r;
			orderCount ++;

			v = order[ orderFirst ];
			orderFirst ++;

			cargo[ r ] = p;
			do { /* while( v != s ) */
				if ( v != r ) {
					incomFlow[ v ] -= cargo[ v ];
					outgoFlow[ v ] -= cargo[ v ];
					flow[ v ] -= cargo[ v ];
					if ( flow[ v ] == 0 ) {
						stek[ stekCount ] = v;
						stekCount ++;
					}
				} /* if( v != r ) */

				if ( v == s ) {
					cargo[ v ] = 0;
				} /* if( v == s ) */
				else {
					int ofs;

					CvGraphVtx* hlpGraphVtx = cvGetGraphVtx( hlpGraph, v );
					CvGraphEdge* hlpGraphEdge = hlpGraphVtx -> first;
					CvGraphEdge* hlpGraphEdge2 = NULL;
					while ( hlpGraphEdge && cargo[ v ] > 0 ) {
						u = cvGraphVtxIdx( hlpGraph,
										   hlpGraphEdge -> vtx[ 1 ] );
						ofs = u == v;

						if ( ofs ) {
							u = cvGraphVtxIdx( hlpGraph,
											   hlpGraphEdge -> vtx[ 0 ] );

							if ( cargo[ u ] == 0 ) {
								order[ orderCount ] = u;
								orderCount ++;
							}

							int delta = ( ( Edge* )hlpGraphEdge ) -> weight
										- ( ( Edge* )hlpGraphEdge ) -> flow;

							delta = CV_MIN2( cargo[ v ], delta );

							(( ( Edge* )hlpGraphEdge ) -> flow) += delta;

							cargo[ v ] -= delta;
							cargo[ u ] += delta;

							if ( ( ( Edge* )hlpGraphEdge ) -> weight ==
									( ( Edge* )hlpGraphEdge ) -> flow ) {
								hlpGraphEdge2 = hlpGraphEdge -> next[ ofs ];
								CvGraphEdge* graphEdge =
									cvFindGraphEdge( graph, u, v );
								( ( Edge* )graphEdge ) -> flow +=
									( ( Edge* )hlpGraphEdge ) -> flow;
								cvGraphRemoveEdgeByPtr( hlpGraph,
														hlpGraphEdge -> vtx[0],
														hlpGraphEdge -> vtx[1] );
							}
						} /* if( ofs ) */

						if ( hlpGraphEdge2 ) {
							hlpGraphEdge = hlpGraphEdge2;
							hlpGraphEdge2 = NULL;
						} else {
							hlpGraphEdge = hlpGraphEdge -> next[ ofs ];
						}
					} /* while( hlpGraphEdge && cargo[ v ] > 0 ) */

				} /* if( v == s ) else */

				v = order[ orderFirst ]; //added
				orderFirst ++; //added

			} while ( v != s ); /* do */

		} /* if( hlpGraph -> total > 2 ) */

	} /* while( hlpGraph -> total > 2 ) */

} /* makePseudoMaxFlow */


// function oneStep produces one alpha-beta exchange for one line of images
//      leftLine        - pointer to the left image line
//      rightLine       - pointer to the right image line
//      alpha           - label number one
//      beta            - label number two
//      corr            - pointer to correspondence array for this line
//      width           - width of image line in pixels
//      mem             - pointer to memory allocated by allocTempMem function
//      vertices        - pointer to vertices array allocated by allocTempMem
//                        function
//      storage         - pointer to CvMemStorage structure
bool oneStep( unsigned char* leftLine,
			  unsigned char* rightLine,
			  int alpha,
			  int beta,
			  int* corr,
			  int width,
			  int* mem,
			  int* vertices,
			  CvMemStorage* storage ) {
	CvGraph* graph = NULL;
	CvGraph* hlpGraph = NULL;
	CvMemStoragePos storagePos;
	int i;
	bool change = false;
	cvSaveMemStoragePos( storage, &storagePos );

	int verticesCount;

	makeGraph( &graph, leftLine, rightLine, alpha, beta, corr, width, storage );

	int s = graph -> total - 2; /* source vertex - alpha vertex */
	//int t = graph -> total - 1; /* terminate vertex - beta vertex */

	int length = makeHelpGraph( graph,
								&hlpGraph,
								storage,
								mem,
								vertices,
								&verticesCount,
								width );
	while ( length != INT_INFINITY ) {
		change = true;
		makePseudoMaxFlow( graph,
						   hlpGraph,
						   vertices,
						   verticesCount,
						   mem,
						   width );
		cvClearGraph( hlpGraph );
		length = makeHelpGraph( graph,
								&hlpGraph,
								storage,
								mem,
								vertices,
								&verticesCount,
								width );
	} /* while( length != INT_INFINITY ) */

	int coord;
	CvGraphVtx* graphVtx;
	for ( i = 0; i < s; i ++ ) {
		CvGraphEdge* graphEdge = cvFindGraphEdge( graph, s, i );

		if ( ( ( Edge* )graphEdge ) -> weight ==
				( ( Edge* )graphEdge ) -> flow ) {
			/* this vertex must have alpha label */
			graphVtx = cvGetGraphVtx( graph, i );
			coord = ( ( Vertex* )graphVtx )-> coord;
			if ( corr[ coord ] != alpha ) {
				corr[ coord ] = alpha; //added
				change = true;
			} else {
				corr[ coord ] = alpha;
			}
		} /* if( ( ( Edge* )graphEdge ) -> weight == ... */
		else {
			/* this vertex must have beta label */
			graphVtx = cvGetGraphVtx( graph, i );
			coord = ( ( Vertex* )graphVtx )-> coord;
			if ( corr[ coord ] != beta ) {
				corr[ coord ] = beta; //added
				change = true;
			} else {
				corr[ coord ] = beta;
			}
		} /* if( ( ( Edge* )graphEdge ) -> weight == ... else */

	} /* for( i = 0; i < s; i ++ ) */

	cvClearGraph( hlpGraph );
	cvClearGraph( graph );

	cvRestoreMemStoragePos( storage, &storagePos );

	return change;

} /* oneStep */

// function initCorr fills correspondence array with initial values
//      corr                - pointer to correspondence array for this line
//      width               -  width of image line in pixels
//      maxPixelDifference  - maximum value of difference between the same
//                            point painted on two images
void initCorr( int* corr, int width, int maxPixelDifference ) {
	int i;
	int pixelDifferenceRange = maxPixelDifference * 2 + 1;

	for ( i = 0; i < width; i ++ ) {
		corr[ i ] = i % pixelDifferenceRange - maxPixelDifference;
	}
} /* initCorr */

// function oneLineCorr fully computes one line of images
//      leftLine                - pointer to the left image line
//      rightLine               - pointer to the right image line
//      corr                    - pointer to the correspondence array for one
//                                line
//      mem                     - pointer to memory allocated by allocTempMem
//                                function
//      vertices                - pointer to memory allocated by allocTempMem
//                                function
//      width                   - width of image line in pixels
//      maxPixelDifference      - maximum value of pixel differences in pixels
//      storage                 - pointer to CvMemStorage struct which
//                                contains memory storage
void oneLineCorr( unsigned char* leftLine,
				  unsigned char* rightLine,
				  int* corr,
				  int* mem,
				  int* vertices,
				  int width,
				  int maxPixelDifference,
				  CvMemStorage* storage ) {
	int result = 1;
	int count = 0;
	int i, j;

	initCorr( corr, width, maxPixelDifference );
	while ( result ) {
		result = 0;

		for ( i = - maxPixelDifference; i < maxPixelDifference; i ++ )
			for ( j = i + 1; j <= maxPixelDifference; j ++ ) {
				result += (int)oneStep( leftLine,
										rightLine,
										i,
										j,
										corr,
										width,
										mem,
										vertices,
										storage );
			} /* for( j = i + 1; j < width; j ++ ) */

		count ++;
		if ( count > /*0*//*1*/2 ) {
			break;
		}

	} /* while( result ) */

} /* oneLineCorr */

// function allLinesCorr computes all lines on the images
//      leftImage           - pointer to the left image
//      leftLineStep        - size of line on the left image in bytes
//      rightImage          - pointer to the right image
//      rightLineStep       - size of line on the right image in bytes
//      width               - width of line in pixels
//      height              - height of image in pixels
//      corr                - pointer to correspondence array for all lines
//      maxPixelDifference  - maximum value of difference between the same
//                            point painted on two images
//      storage             - pointer to CvMemStorage which contains memory
//                            storage
void allLinesCorr( unsigned char* leftImage,
				   int leftLineStep,
				   unsigned char* rightImage,
				   int rightLineStep,
				   int width,
				   int height,
				   int* corr,
				   int maxPixelDifference,
				   CvMemStorage* storage ) {
	int i;
	unsigned char* leftLine = leftImage;
	unsigned char* rightLine = rightImage;
	int* mem;
	int* vertices;

	allocTempMem( &mem,
				  &vertices,
				  width );

	for ( i = 0; i < height; i ++ ) {
		oneLineCorr( leftLine,
					 rightLine,
					 corr + i * width,
					 mem,
					 vertices,
					 width,
					 maxPixelDifference,
					 storage );
		leftLine += leftLineStep;
		rightLine += rightLineStep;
	} /* for( i = 0; i < height; i ++ ) */

	freeTempMem( &mem,
				 &vertices );

} /* allLinesCorr */

// This function produces morphing of two images into one image, which includes morphed
// image or depth map
//      _leftImage              - pointer to left image
//      _leftLineStep           - size of line on left image in bytes
//      _rightImage             - pointer to right image
//      _rightLineStep          - size of line on right image in bytes
//      _resultImage            - pointer to result morphed image
//      _resultLineStep         - size of line on result image in bytes
//      _corrArray              - pointer to array with correspondences
//      _numCorrArray           - pointer to array with numbers correspondeces on each line
//      width                   - width of images
//      height                  - height of images
//      alpha                   - position of virtual camera ( 0 corresponds to left image, 1 - to right one )
//      imageNeed               - defines your wishes. if you want to see normal morphed image you have to set
//                                this parameter to morphNormalImage ( this is default value ), else if you want
//                                to see depth map you have to set this parameter to morphDepthMap and set the
//                                next parameter ( maxPixelDifference ) to real value
//      maxPixelDifference      - maximum value of pixel difference on two images
void CCvGraphCutMorpher::Morph( unsigned char* _leftImage,
								int _leftLineStep,
								unsigned char* _rightImage,
								int _rightLineStep,
								unsigned char* _resultImage,
								int _resultLineStep,
								int* _corrArray,
								int width,
								int height,
								float alpha,
								morphImageType imageNeed,
								int maxDifference
							  ) {
	unsigned char* leftArray    = _leftImage;
	unsigned char* middleArray  = _resultImage;
	unsigned char* rightArray   = _rightImage;
	int leftLineSize            = _leftLineStep;
	int middleLineSize          = _resultLineStep;
	int rightLineSize           = _rightLineStep;

	int lineNumber;
	unsigned char* leftTemp;
	unsigned char* middleTemp;
	unsigned char* rightTemp;
	int leftPixel;
	int prevLeftPixel;
	int middlePixel;
	int prevMiddlePixel;
	int rightPixel;
	int prevRightPixel;
	int leftPixel3;
	int middlePixel3;
	int rightPixel3;
	int i;
	int j;
	int tempIndex;
	int* result;
	int number;
	float alpha1        = 1.0f - alpha;

	for ( lineNumber = 0; lineNumber < height; lineNumber ++ ) {
		leftTemp    = leftArray + leftLineSize * lineNumber;
		middleTemp  = middleArray + middleLineSize * lineNumber;
		rightTemp   = rightArray + rightLineSize * lineNumber;
		memset( ( void* )middleTemp, 0, middleLineSize );

		result = _corrArray + width * lineNumber;
		number = width;

		prevLeftPixel   = 0;
		prevRightPixel  = prevLeftPixel + result[ 0 ];
		if ( prevRightPixel >= width ) {
			prevRightPixel = width - 1;
		} else if ( prevRightPixel < 0 ) {
			prevRightPixel = 0;
		}
		prevMiddlePixel =
			(int)( prevLeftPixel * alpha1 + prevRightPixel * alpha );
		for ( i = 0; i < number - 1; i ++ ) {
			leftPixel       = i;
			rightPixel      = i + result[ i ];
			if ( rightPixel >= width ) {
				rightPixel = width - 1;
			} else if ( rightPixel < 0 ) {
				rightPixel = 0;
			}
			middlePixel     =
				(int)( leftPixel * alpha1 + rightPixel * alpha );
			leftPixel3      = leftPixel * 3;
			middlePixel3    = middlePixel * 3;
			rightPixel3     = rightPixel * 3;

			if ( imageNeed == morphDepthMap ) {
				int t   = leftPixel - rightPixel + maxDifference;
				t       = t < 0 ? -t : t;
				t       = t * 255 / maxDifference / 2;
				middleTemp[ middlePixel3 ]      = ( unsigned char )t;
				middleTemp[ middlePixel3 + 1 ]  = ( unsigned char )t;
				middleTemp[ middlePixel3 + 2 ]  = ( unsigned char )t;
			} // if( imageNeed == morphDepthMap )
			else {
				middleTemp[ middlePixel3 ] =
					(unsigned char)( leftTemp[ leftPixel3 ] * alpha1
									 + rightTemp[ rightPixel3 ] * alpha );
				middleTemp[ middlePixel3 + 1 ] =
					(unsigned char)( leftTemp[ leftPixel3 + 1 ] * alpha1
									 + rightTemp[ rightPixel3 + 1 ] * alpha );
				middleTemp[ middlePixel3 + 2 ] =
					(unsigned char)( leftTemp[ leftPixel3 + 2 ] * alpha1
									 + rightTemp[ rightPixel3 + 2 ] * alpha );

				if ( middlePixel - prevMiddlePixel > 1 ) { // occlusion
					if ( leftPixel - prevLeftPixel > 1 ) {
						int LenSrc  = leftPixel - prevLeftPixel - 2;
						int LenDest = middlePixel - prevMiddlePixel - 1;
						for ( j = prevMiddlePixel + 1; j < middlePixel; j ++ ) {
							tempIndex   = prevLeftPixel + 1 + LenSrc *
										  ( j - prevMiddlePixel - 1 ) / LenDest;
							middleTemp[ j * 3 ]     =
								leftTemp[ tempIndex * 3 ];
							middleTemp[ j * 3 + 1 ] =
								leftTemp[ tempIndex * 3 + 1 ];
							middleTemp[ j * 3 + 2 ] =
								leftTemp[ tempIndex * 3 + 2 ];
						}
					} // if( leftPixel - prevLeftPixel > 1 )
					else {
						int LenSrc  = rightPixel - prevRightPixel - 2;
						int LenDest = middlePixel - prevMiddlePixel - 1;
						for ( j = prevMiddlePixel + 1; j < middlePixel; j ++ ) {
							tempIndex   = prevRightPixel + 1 + LenSrc *
										  ( j - prevMiddlePixel - 1 ) / LenDest;
							middleTemp[ j * 3 ]     =
								rightTemp[ tempIndex * 3 ];
							middleTemp[ j * 3 + 1 ] =
								rightTemp[ tempIndex * 3 + 1 ];
							middleTemp[ j * 3 + 2 ] =
								rightTemp[ tempIndex * 3 + 2 ];
						}
					} // if( leftPixel - prevLeftPixel > 1 ) else

				} // if( middlePixel - prevMiddlePixel > 1 )

			} // if( imageNeed == morphDepthMap ) else

			if ( middlePixel > prevMiddlePixel ) {
				if ( leftPixel > prevLeftPixel ) {
					prevLeftPixel   = leftPixel;
				}
				if ( rightPixel > prevRightPixel ) {
					prevRightPixel  = rightPixel;
				}
				prevMiddlePixel = middlePixel;
			}
		} // for( i = number - 1; i >= 0; i -- )

	} // for( lineNumber = 0; lineNumber < LeftImage -> m_Raster -> GetHeight() )

} // Morph

bool  CCvGraphCutMorpher::OnCalculateStereo() {
	CvSize imageSizeLeft = GetImageSize( m_left_img ),
		   imageSizeRight = GetImageSize( m_right_img );

	if ( ( imageSizeLeft.width != imageSizeRight.width )
			|| ( imageSizeLeft.height != imageSizeRight.height ) ) {
		return false;
	}

	if ( m_corr ) {
		free( m_corr );
		m_corr = NULL;
	}
	m_corr = ( int* ) malloc( m_left_img -> width
							  * m_left_img -> height
							  * sizeof( int ) );

	if ( !m_storage ) {
		m_storage = cvCreateMemStorage( 0 );
		m_isStorageAllocated = true;
	}
	// Find correspondence for full image and store it to corr array
	allLinesCorr( ( unsigned char* )m_left_img -> imageData,
				  m_left_img -> widthStep,
				  ( unsigned char* )m_right_img -> imageData,
				  m_right_img -> widthStep,
				  m_left_img -> width,
				  m_left_img -> height,
				  m_corr,
				  m_maxPixelDifference,
				  m_storage );

	m_isStereoReady = true;

	return true;
}

bool  CCvGraphCutMorpher::OnCalculateVirtualImage() {
	// Output image to ResultImage window
	Morph( ( unsigned char* )m_left_img -> imageData,
		   m_left_img ->widthStep,
		   ( unsigned char* )m_right_img -> imageData,
		   m_right_img -> widthStep,
		   ( unsigned char* )m_virtual_img -> imageData,
		   m_virtual_img -> widthStep,
		   m_corr,
		   m_left_img -> width,
		   m_left_img -> height,
		   m_pan );

	m_isVirtualImageReady = true;

	return true;
}

bool  CCvGraphCutMorpher::OnCalculateDisparity() {
	Morph( ( unsigned char* )m_left_img -> imageData,
		   m_left_img ->widthStep,
		   ( unsigned char* )m_right_img -> imageData,
		   m_right_img -> widthStep,
		   ( unsigned char* )m_disparity_img -> imageData,
		   m_disparity_img -> widthStep,
		   m_corr,
		   m_left_img -> width,
		   m_left_img -> height,
		   m_pan,
		   morphDepthMap,
		   m_maxPixelDifference );

	return true;
}

bool  CCvGraphCutMorpher::OnCalculateDisparityImage() {
	Morph( ( unsigned char* )m_left_img -> imageData,
		   m_left_img ->widthStep,
		   ( unsigned char* )m_right_img -> imageData,
		   m_right_img -> widthStep,
		   ( unsigned char* )m_disparity_img -> imageData,
		   m_disparity_img -> widthStep,
		   m_corr,
		   m_left_img -> width,
		   m_left_img -> height,
		   m_pan,
		   morphDepthMap,
		   m_maxPixelDifference );

	return true;
}

CCvGraphCutMorpher::CCvGraphCutMorpher() {
	m_maxPixelDifference = MAX_DIFFERENCE;
	m_corr = 0;
	m_isStereoReady = false;
	m_isVirtualImageReady = false;
	m_isDisparityReady = false;
	m_storage = NULL;
	m_isStorageAllocated = false;
}

#endif

/* End of file */
